

#include "qelib.h"
#include "gev.h"
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <arpa/inet.h>
#include <sys/socket.h>
#include <netinet/in.h>
#include <time.h>
#include <sys/time.h>
#include <fcntl.h>
#include <unistd.h>


#define GEV_LOGNAME            "gev-app"
#define gevapp_debug(...)      qelog_debug(GEV_LOGNAME,   __VA_ARGS__)
#define gevapp_info(...)       qelog_info(GEV_LOGNAME,    __VA_ARGS__)
#define gevapp_notice(...)     qelog_notice(GEV_LOGNAME,  __VA_ARGS__)
#define gevapp_warning(...)    qelog_warning(GEV_LOGNAME, __VA_ARGS__)
#define gevapp_error(...)      qelog_error(GEV_LOGNAME,   __VA_ARGS__)


static int req_id = 0;

typedef struct {
    int fd;
    int discover_times;
    int discover_max;
    qe_workqueue *wq;
    struct gev_connect *connects;
    int should_stop:1;
} gev_app_ctx_t;

static qe_ret setnonblocking(int sockfd)
{
    int flag = fcntl(sockfd, F_GETFL, 0);
    if (flag < 0) {
        gevapp_error("Get sockfd flag error");
        return qe_err_io;
    }

    if (fcntl(sockfd, F_SETFL, flag | O_NONBLOCK) < 0) {
        gevapp_error("Set sock nonblocking error");
        return qe_err_io;
    }

    return qe_ok;
}

static qe_ret gev_device_discover(gev_app_ctx_t *ctx)
{
    int fd;
    int enable = 1;
    int recvlen;
    qe_ret ret;
    struct sockaddr_in addr, dev_addr;
    socklen_t len = sizeof(dev_addr);
    struct timeval tv = {10, 0};
    gvcp_cmd_header_t discovery_message;
    gvcp_discovery_ack_t *ack;
    char recv_buf[1024];

    fd = socket(AF_INET, SOCK_DGRAM, 0);
    if (fd < 0) {
        gevapp_error("Failed to create socket");
        return qe_err_io;
    }

    if (setsockopt(fd, SOL_SOCKET, SO_BROADCAST, &enable, sizeof(enable)) < 0) {
        gevapp_error("Failed to enable boardcast");
        ret = qe_err_io;
        goto __error;
    }

    //setnonblocking(fd);

    if (setsockopt(fd, SOL_SOCKET, SO_RCVTIMEO, (char *)&tv, sizeof(tv)) < 0) {
        gevapp_error("Failed to set timeout");
        ret = qe_err_io;
        goto __error;
    }

    qe_memset(&addr, 0x0, sizeof(addr));
    addr.sin_family = AF_INET;
    addr.sin_addr.s_addr = inet_addr("127.0.0.1");
    addr.sin_port = htons(GVCP_PORT);

    discovery_message.key = GVCP_MSG_KEY;
    discovery_message.flag |= GVCP_FLAG_ACKNOWLEDGE;
    discovery_message.command = GEV_DISCOVERY_CMD;
    discovery_message.length = 0;
    discovery_message.req_id = req_id;
    if (sendto(fd, &discovery_message, sizeof(discovery_message), 0, 
        (struct sockaddr *)&addr, sizeof(addr)) < 0) {
        gevapp_error("Failed to send broadcast");
        ret = qe_err_io;
        goto __error;
    }

    recvlen = recvfrom(fd, recv_buf, sizeof(recv_buf), 0, (struct sockaddr *)&dev_addr, &len);
    if (recvlen < 0) {
        gevapp_error("Failed to recv ack");
        ret = qe_err_io;
        goto __error;
    }

    ack = (gvcp_discovery_ack_t *)recv_buf;
    //ack->head.answer = ntohs(ack->head.answer);
    //ack->head.ack_id = ntohs(ack->head.ack_id);
    //ack->head.status = ntohs(ack->head.status);
    
    if (ack->head.answer != GEV_DISCOVERY_ACK) {
        gevapp_error("Answer not right %d", ack->head.answer);
        ret = qe_err_match;
        goto __error;
    }

    if (ack->head.ack_id != req_id) {
        gevapp_error("Ack id not right %d", ack->head.ack_id);
        ret = qe_err_match;
        goto __error;
    }

    gevapp_info("receive discover ack");

    if (ack->head.status != GEV_STATUS_SUCCESS) {
        gevapp_error("Status error %d", ack->head.status);
        ret = qe_err_common;
        goto __error;
    }

    gevapp_info("Device discovery success");
    gevapp_info("version     : %d.%d", ack->spec_version_major, ack->spec_version_minor);
    gevapp_info("device mode : %d", ack->device_mode);
    gevapp_info("current ip  : %p", ack->current_ip);
    gevapp_info("netmask     : %p", ack->current_subnet_mask);
    gevapp_info("gateway     : %p", ack->default_gateway);
    gevapp_info("manu name   : %s", ack->manufacturer_name);
    gevapp_info("manu info   : %s", ack->manufacturer_specific_information);
    gevapp_info("model name  : %s", ack->model_name);
    gevapp_info("device ver  : %s", ack->device_version);
    gevapp_info("device ver  : %s", ack->device_version);
    gevapp_info("serial num  : %s", ack->serial_number);
    gevapp_info("username    : %s", ack->user_defined_name);

__exit:
    ctx->fd = fd;
    return qe_ok;

__error:
    close(fd);
    return ret;
}

static qe_ret work_device_discover(qe_workqueue *queue, struct qe_worker *worker)
{
    qe_ret ret;
    gev_app_ctx_t *ctx = (gev_app_ctx_t *)worker->data;

    gevapp_debug("work device discover");

    if (ctx->discover_times >= ctx->discover_max) {
        gevapp_info("device discover upto max time %d", ctx->discover_times);
        return qe_ok;
    }

    ret = gev_device_discover(ctx);
    if (ret != qe_ok) {
        gevapp_info("device discover error:%d", ret);
        gevapp_info("device discover retry after %d(s)", 15);
        ctx->discover_times++;
        return qe_workqueue_schedule(queue, work_device_discover, 15*1000, worker->data);
    }

    return qe_ok;
}

qe_bool app_should_stop(gev_app_ctx_t *ctx)
{
    return (ctx->should_stop == 1);
}

int main(int argc, char *argv[])
{
    qelog_init(QELOG_DEBUG, QELOG_HMS | QELOG_DM | QELOG_LV | QELOG_CL);

    gevapp_debug("Gev App Start");

    /* GVCP device discovery */
    gev_app_ctx_t *ctx = qe_malloc(sizeof(gev_app_ctx_t));
    if (!ctx) {
        gevapp_error("ctx alloc error");
        return -1;
    }

    ctx->discover_max = 30;

    ctx->wq = qe_workqueue_new(32);

    qe_workqueue_schedule(ctx->wq, work_device_discover, 1, ctx);

    gevapp_debug("loop");

    do {
        qe_workqueue_service(ctx->wq, qe_time_ms());
    } while (!app_should_stop(ctx));

    return -1;
}